#include "motor.h"
#include "stm32f4xx_hal_can.h"

moto_info_t motor_info[7];

void set_motor_voltage(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4)
{
  CAN_TxHeaderTypeDef tx_header;
  uint8_t             tx_data[8];
    
  tx_header.StdId = (id_range == 0)?(0x200):(0x1ff);
  tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 8;

  tx_data[0] = (v1>>8)&0xff;
  tx_data[1] =    (v1)&0xff;
  tx_data[2] = (v2>>8)&0xff;
  tx_data[3] =    (v2)&0xff;
  tx_data[4] = (v3>>8)&0xff;
  tx_data[5] =    (v3)&0xff;
  tx_data[6] = (v4>>8)&0xff;
  tx_data[7] =    (v4)&0xff;
  HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
}

void delay_us(uint16_t xus)
{
	for(uint16_t i = xus;i > 0;i--)
	{
		for(uint8_t j = 0;j < 168;j++);
	}
}

void delay_ms(uint16_t xms)
{
	for(uint16_t i = xms;i > 0;i--)
	{
		delay_us(1000);
	}
}
int32_t motor_round(int16_t angle_new)
{
	static int16_t  angle_last;
	static int n=0;
	static uint8_t init_flag = 1;
	int32_t angle_abs = 0;
	
	if(init_flag)
	{
		init_flag = 0;
		angle_last = angle_new;
	}

   if(angle_new-angle_last<-4096)
	 {
		 n++;
	 }
	 if(angle_new-angle_last>4096)
	 {
		 n--;
	 }
	 
	 angle_abs =angle_new+n*8192;
	 angle_last=angle_new;
	 return angle_abs ;
}